#include <iostream>
#include "tocUtil.h"

using namespace std;

TOCHuman::TOCHuman(void) : TOCBody ()
{
	this->rgb[0] = 0.0f;
	this->rgb[1] = 1.0f;
	this->rgb[2] = 0.0f;
}

TOCHuman::TOCHuman(GLuint uuid) : TOCBody ()
{
	this->uuid = uuid;

	// TODO: Remove redudancy;
	this->rgb[0] = 0.0f;
	this->rgb[1] = 1.0f;
	this->rgb[2] = 0.0f;
}


TOCHuman::~TOCHuman(void) {}

void TOCHuman::_drawMe(void) 
{
	GLfloat e = 0.0;
	const GLfloat halfLength  = 0.002;
	const GLfloat halfBreadth = 0.001;

	glColor3fv(this->rgb);

	drawTriangle(this->getLook(), 
		     this->getX(), this->getY(), 
		     halfLength, halfBreadth, e);
}

void TOCHuman::advance(void)
{
	switch (this->getLook()) {
	case NORTH:
		this->setXY(this->getX(), this->getY() + this->getSpeed());
		break;

	case EAST:
		this->setXY(this->getX() + this->getSpeed(), this->getY());
		break;

	case SOUTH:
		this->setXY(this->getX(), this->getY() - this->getSpeed());
		break;

	case WEST:
		this->setXY(this->getX() - this->getSpeed(), this->getY());
		break;
	}
}

int count = 0;

bool TOCHuman::isForbiddenArea(void)
{		
	GLfloat c[4];
	GLint v[4];
	GLdouble m[16], p[16];

	GLdouble xx, yy, zz;
	glGetDoublev(GL_MODELVIEW_MATRIX, m);
	glGetDoublev(GL_PROJECTION_MATRIX, p);
	glGetIntegerv(GL_VIEWPORT, v);

	gluProject(this->getX(), this->getY(), 1.0, m, p, v, &xx, &yy, &zz);

	// glLoadIdentity();
	glReadPixels(xx, yy, 1, 1, GL_RGB, GL_FLOAT, c);
	// cout << xx << ", " << yy << c[0] << ", " << c[1] << ", " << c[2] << endl;

	if (c[0] <= 0.7f && c[1] <= 0.7f && c[2] <= 0.7f) {
		// cout << c[0] << ", " << c[1] << ", " << c[2] << endl;
		// count++;
		// this->turn(RIGHT);
		// advance();
		return true;
	}

	// cout << "normal: " << c[0] << ", " << c[1] << ", " << c[2] << endl;
	return false;
}

void TOCHuman::_computeMove(void)
{
	// cout << "count: " << count << endl;
	// do {
	// 	this->turn(RIGHT);
	// 	this->_drawMe();
	// 	advance();
	// } while(isForbiddenArea());

	advance();

	if (count > 1000)
		exit(0);
}
